#include <opencv2/opencv.hpp>
#include "RMVideoCapture.hpp"
#include "iostream"
using namespace cv;
using namespace std;

#define DEBUG_DETECT
int iLowH = 41;int iHighH = 130;
int iLowS = 0;int iHighS = 255;
int iLowV = 210;int iHighV = 255;
int gray_th = 43;
int color_th = 59;
int main()
{
#ifdef RM
    VideoCapture cap(1);
#else
    RMVideoCapture cap("/dev/video1");
    cap.setVideoFormat(640, 360, 1);
    cap.setExposureTime(0, 64);//settings->exposure_time);
    cap.startStream();
    cap.info();
#endif
    Mat src;
    bool using_hsv = 1, color = 0;
    Mat mask, binary_brightness_img, binary_color_img;
    Mat gray;
    Mat result_img;

    cv::namedWindow("Control", CV_WINDOW_NORMAL);
    cv::createTrackbar("LowH", "Control", &iLowH, 255); //Hue (0 - 179)
    cv::createTrackbar("HighH", "Control", &iHighH, 255);
    cv::createTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
    cv::createTrackbar("HighS", "Control", &iHighS, 255);
    cv::createTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
    cv::createTrackbar("HighV", "Control", &iHighV, 255);
    cv::createTrackbar("gray_th", "Control", &gray_th, 255);
    cv::createTrackbar("color_th", "Control", &color_th, 255);
    while(1)
    {
        cap >> src;
        cvtColor(src,gray,COLOR_BGR2GRAY);
        if(using_hsv)
        {
            Mat imgHSV;
            cvtColor(src,imgHSV,COLOR_BGR2HSV);
            if(!color)   //blue
            {
                inRange(imgHSV, Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV), mask);
//                inRange(imgHSV, Scalar(40,0,200), Scalar(100,255,255), mask);
            }else   //red
            {
                Mat1b mask1, mask2;
                inRange(imgHSV, Scalar(0,0,62), Scalar(25,255,255), mask1);
                //            inRange(imgHSV, Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV), mask2);
                inRange(imgHSV, Scalar(173,0,200), Scalar(193,255,255), mask2);
                imshow("mask1",mask1);
                imshow("m2",mask2);
                mask = mask1| mask2;
            }
            //        inRange(imgHSV, Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV), mask);
            Mat kernel = getStructuringElement(MORPH_CROSS,Size(3,3));
            morphologyEx(mask, mask, MORPH_CLOSE, kernel);
#ifdef DEBUG_DETECT
            imshow("mask", mask);

#endif
            vector<vector<Point>> contour;
            vector<Vec4i> hier;
            findContours(mask, contour, hier, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

            for(size_t i=0; i < contour.size(); i++)
            {
                if(hier.at(i)[3] != -1) continue;
                double length = arcLength(contour[i],true);
                if(length > 5 && length < 10000)
                {
                    RotatedRect rotate_rect = minAreaRect(contour[i]);
                    if(rotate_rect.size.height < rotate_rect.size.width)
                    {
                        rotate_rect.angle += 90;
                        double tmp = rotate_rect.size.height;
                        rotate_rect.size.height = rotate_rect.size.width;
                        rotate_rect.size.width = tmp;
                    }
                    Point2f rect_point[4];
                    rotate_rect.points(rect_point);
                    for(int i = 0; i < 4; i++)
                    {
                        line(src, Point_<int>(rect_point[i]),
                             Point_<int>(rect_point[(i+1)%4]),
                                Scalar(0,0,255));
                    }

                }
            }
        }else
        {
            Mat element = getStructuringElement(MORPH_RECT, Size(3,3));
            dilate(src, src, element);
            vector<cv::Mat> bgr;
            split(src, bgr);
            Mat result_img;

            if(color == 1)
            {
                subtract(bgr[2], bgr[1], result_img);
            }else
            {
                subtract(bgr[0], bgr[2], result_img);
            }
            threshold(gray, binary_brightness_img, gray_th, 255, CV_THRESH_BINARY);
            threshold(result_img, binary_color_img, color_th, 255, CV_THRESH_BINARY);

#ifdef DEBUG_DETECT
            imshow("binary_brightness_img", binary_brightness_img);
            imshow("binary_color_img", binary_color_img);
#endif
            vector<vector<Point>> contours_light;
            vector<vector<Point>> contours_brightness;
            findContours(binary_color_img, contours_light, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
            findContours(binary_brightness_img, contours_brightness, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
            for(unsigned int i = 0; i < contours_brightness.size(); ++i)
            {
                for(unsigned int j = 0; j < contours_light.size(); ++j)
                {
                    if(pointPolygonTest(contours_light[j], contours_brightness[i][0], false) >= 0.0 )
                    {
                        double length = arcLength(Mat(contours_brightness[i]),true);
                        if(length > 10 && length < 5000)
                        {
                            RotatedRect rotate_rect = minAreaRect(contours_brightness[i]);
                            //rotated Rect angle transform
                            if(rotate_rect.size.height < rotate_rect.size.width)
                            {
                                rotate_rect.angle += 90;
                                double tmp = rotate_rect.size.height;
                                rotate_rect.size.height = rotate_rect.size.width;
                                rotate_rect.size.width = tmp;
                            }
                            Point2f rect_point[4];
                            rotate_rect.points(rect_point);
                            for(int i = 0; i < 4; i++)
                            {
                                line(src, Point_<int>(rect_point[i]),
                                     Point_<int>(rect_point[(i+1)%4]),
                                        Scalar(0,0,255));
                            }
                        }
                    }
                }
            }


        }
        imshow("image", src);
        if(waitKey(1)>0)
            break;
    }


    return 0;
}


